import os
import launch.event_handlers
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import ExecuteProcess

def generate_launch_description():
    # 获取 URDF 文件路径
    urdf_package_path = get_package_share_directory("description_pkg")
    urdf_model_path = os.path.join(urdf_package_path, "urdf","agv","agv.urdf.xacro")
   
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    # 为 Launch 声明参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model', 
        default_value=str(urdf_model_path),
        description='URDF 的绝对路径')
    # 获取文件内容生成新的参数
    robot_description = ParameterValue(
        launch.substitutions.Command(
            ['xacro ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str)
   
   
    # 请求 Gazebo 加载机器人
    spawn_entity_node = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        output='screen',
        arguments=['-topic', '/robot_description',
                   '-entity', 'agv_robot',
                   '-x', '0',
                   '-y', '0',
                   '-z', '0',
                   '-Y', '0',
                   ])
    
    robot_state_publisher_cmd = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name='robot_state_publisher',
        output='screen',
        parameters=[{
                'use_sim_time': use_sim_time,
                'robot_description': robot_description
            }],
    )

    joint_state_publisher_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
         parameters=[{
                'use_sim_time': use_sim_time,
                'robot_description': robot_description
            }],
    )
    # 加载控制器
    load_joint_state_broadcaster = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'joint_state_controller'],
        output='screen'
    )

    load_arm_connect_joint_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'arm_connect_joint_controller'],
        output='screen'
    )

    ld = LaunchDescription()
    # gazebo 仿真
    ld.add_action(action_declare_arg_mode_path)
    ld.add_action(launch.actions.TimerAction(period=2.0,actions=[spawn_entity_node]))
    ld.add_action(launch.actions.TimerAction(period=3.0,actions=[robot_state_publisher_cmd]))
    # ld.add_action(joint_state_publisher_node)
    ld.add_action(launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=spawn_entity_node,
            on_exit=[load_joint_state_broadcaster],
        )
    ))
    ld.add_action(launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=load_joint_state_broadcaster,
            on_exit=[load_arm_connect_joint_controller],
        )
    ))
    return ld
